/*
 * @Date: 2020-10-05 03:38:22
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-19 22:55:30
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/UWBRosNode.h
 * @Description: uwb ros node 管理类头文件
 */
#ifndef INCLUDE_UWBROSNODE_H
#define INCLUDE_UWBROSNODE_H

#include "../../include/MsgLink/UWBManager.h"
#include <px4_swarm_control/data_buffer.h>
#include <px4_swarm_control/incoming_broadcast_data.h>
#include <px4_swarm_control/remote_uwb_info.h>
#include <ros/ros.h>
#include <sensor_msgs/TimeReference.h>
#include <std_msgs/Header.h>
#include <unistd.h>
#include <mutex>
#include <unordered_set>
#include <csignal>
using namespace px4_swarm_control;

#define MAX_SEND_BYTES 40

class UWBRosNode : public UWBManager
{
    ros::Timer fast_timer, slow_timer;
    std::mutex send_lock;
    std::mutex recv_lock;
    double send_freq = 50;
    double recv_freq = 100;
    int send_buffer_size = 80;
    std::string send_topic_name = "";
    bool enable_uwb_send = true;
    bool enable_parse_buffer= false;
    Buffer recv_data_buffer;
    std::unordered_set<int32_t> sent_msgs;

public:
    static UWBRosNode *getInstance();
    void setUWBRosNode(std::string serial_name, int baudrate, ros::NodeHandle nh, bool enable_debug);

protected:
    void
    sendBroadcastDataCallback(const ros::TimerEvent &e);

    void readAndParseCallback(const ros::TimerEvent &e);

    virtual void onSendBroadcastReq(data_buffer msg);

    void onBroadcastDataRecv(int _id, Buffer _msg) override;

    void onSystemTimeUpdate() override;
    void onNodeDataUpdated() override;

private:
    UWBRosNode() = default;
    static UWBRosNode *instance;
    ros::Publisher remote_node_pub, broadcast_data_pub, time_reference_pub;
    ros::Subscriber recv_bdmsg;
    Buffer send_buffer;
    void parseBroadcastDataRecv();
    static void shutdown_handler(int sig);
    static void watchSignal();
};


#endif